#include "term.h"
#include "motor.h"
#include "mqtt_comm.h"
#include <pthread.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <strings.h> // for strcasecmp
#include <unistd.h>

static const double AXIS1_K = 10;

static const double POS_MIN_UM = 5000.0;
static const double POS_MAX_UM = 180000.0;

extern volatile bool g_new_message_flag;

MotorSet cmdSet = {
    {0}, {0}, MODEL_PP, MODEL_PP, {0},
};

pthread_mutex_t cmdSet_mutex = PTHREAD_MUTEX_INITIALIZER;

extern RobotControlData g_robotControlData;
extern RobotStateData g_robotStateData;

static char last_msg_id[128] = "";

extern volatile bool g_quit;

void updateStatues(MotorSet *cmd) {
  if (cmd->mode != cmd->previous_mode) {
    for (int i = 0; i < MOTOR_NUM; i++) {
      cmd->state[i] = 0;
    }
    cmd->previous_mode = cmd->mode;
  }
}

int SetMotor(RobotControlData *recv) {
  int target_motor_pos = 0;
  double target_pos_um = 0;

  if (strcasecmp(recv->move_type, "absolute") == 0) {
    target_pos_um = recv->parameter;
    if (target_pos_um < POS_MIN_UM || target_pos_um > POS_MAX_UM) {
      printf("错误: 绝对位置 %.2f µm 超出范围 [%.2f, %.2f] (status=104)\n",
             target_pos_um, POS_MIN_UM, POS_MAX_UM);
      return 104;
    }
    target_motor_pos = target_pos_um * AXIS1_K;
    printf("设置绝对运动: 目标位置 = %.2f µm (电机值: %d)\n", recv->parameter,
           target_motor_pos);
    cmdSet.target_pos[0] = target_motor_pos;

  } else if (strcasecmp(recv->move_type, "relative") == 0) {
    int current_pos_counts = cmdSet.actual_pos[0];
    double current_pos_um = (double)current_pos_counts / AXIS1_K;
    target_pos_um = current_pos_um + recv->parameter;

    if (target_pos_um < POS_MIN_UM || target_pos_um > POS_MAX_UM) {
      printf("错误: 相对运动后目标位置 %.2f µm 超出范围 [%.2f, %.2f] "
             "(status=104)\n",
             target_pos_um, POS_MIN_UM, POS_MAX_UM);
      return 104;
    }
    int relative_increment = recv->parameter * AXIS1_K;
    target_motor_pos = current_pos_counts + relative_increment;
    printf("设置相对运动: 当前位置 = %.2f µm, 相对增量 = %.2f µm, 目标位置 = "
           "%.2f µm\n",
           current_pos_um, recv->parameter, target_pos_um);
    printf("  (电机值: 当前=%d, 增量=%d, 目标=%d)\n", current_pos_counts,
           relative_increment, target_motor_pos);
    cmdSet.target_pos[0] = target_motor_pos;

  } else if (strcasecmp(recv->move_type, "homing") == 0) {
    printf("设置回零模式: 目标位置 = 0 µm\n");
    cmdSet.target_pos[0] = 0;
  } else if (strcasecmp(recv->move_type, "halt") == 0) {
    printf("设置停止模式: 保持当前位置 = %.2f µm\n",
           cmdSet.actual_pos[0] / AXIS1_K);
    cmdSet.target_pos[0] = cmdSet.actual_pos[0];
  } else if (strcasecmp(recv->move_type, "disable_voltage") == 0) {
    printf("收到急停命令: 准备进入持久急停状态\n");
    return 99;
  } else {
    printf("错误: 未知的move_type='%s' (status=102)\n", recv->move_type);
    return 102;
  }

  return 0;
}

void *process_main(void *arg) {
  while (1) {
    static double last_pos_um = 0;

    if (g_robotStateData.status == 1) { // 空闲状态
      if (g_new_message_flag) {
        g_new_message_flag = false;

        if (g_robotControlData.axis_num != 29) {
          printf("错误: 收到axis_num=%d的命令，不是29 (status=102)\n",
                 g_robotControlData.axis_num);
          g_robotStateData.status = 102;
          if (strlen(g_robotControlData.msg_id) > 0)
            mqtt_send_ack(g_robotControlData.msg_id, 102);
          continue;
        }

        bool is_special_command =
            (strcasecmp(g_robotControlData.move_type, "homing") == 0 ||
             strcasecmp(g_robotControlData.move_type, "halt") == 0 ||
             strcasecmp(g_robotControlData.move_type, "disable_voltage") == 0);

        if (!is_special_command) {
          if (strlen(g_robotControlData.msg_id) == 0) {
            printf("错误: 运动命令缺少msg_id (status=102)\n");
            g_robotStateData.status = 102;
            continue;
          }
          if (strlen(last_msg_id) > 0 &&
              strcmp(g_robotControlData.msg_id, last_msg_id) == 0) {
            printf("警告: 收到重复的msg_id=%s (status=103)\n",
                   g_robotControlData.msg_id);
            g_robotStateData.status = 103;
            continue;
          }
        }

        printf("\n========== 接收到新任务 ==========\n");
        // ... 打印任务信息 ...

        pthread_mutex_lock(&cmdSet_mutex);
        int set_result = SetMotor(&g_robotControlData);

        if (set_result == 102) {
          g_robotStateData.status = 102;
          if (strlen(g_robotControlData.msg_id) > 0)
            mqtt_send_ack(g_robotControlData.msg_id, 102);
        } else if (set_result == 104) {
          g_robotStateData.status = 104;
          if (strlen(g_robotControlData.msg_id) > 0)
            mqtt_send_ack(g_robotControlData.msg_id, 104);
        } else if (set_result == 99) {
          printf("\n========== 执行急停命令 ==========\n");
          g_robotStateData.status = 300;
        } else {
          g_robotStateData.status = 0;
          if (!is_special_command && strlen(g_robotControlData.msg_id) > 0) {
            strcpy(last_msg_id, g_robotControlData.msg_id);
          }
          if (strlen(g_robotControlData.msg_id) > 0)
            mqtt_send_ack(g_robotControlData.msg_id, 0);

          if (strcasecmp(g_robotControlData.move_type, "absolute") == 0) {
            last_pos_um = g_robotControlData.parameter;
          } else if (strcasecmp(g_robotControlData.move_type, "relative") ==
                     0) {
            last_pos_um =
                (cmdSet.actual_pos[0] / AXIS1_K) + g_robotControlData.parameter;
          } else if (strcasecmp(g_robotControlData.move_type, "homing") == 0) {
            last_pos_um = 0;
          } else if (strcasecmp(g_robotControlData.move_type, "halt") == 0) {
            last_pos_um = cmdSet.actual_pos[0] / AXIS1_K;
          }
        }
        pthread_mutex_unlock(&cmdSet_mutex);
      }
    } else if (g_robotStateData.status == 102) { // 轴号或类型错误
      if (g_new_message_flag) {
        printf("收到新指令，准备从错误(102)中恢复并重新评估...\n");
        g_robotStateData.status = 1;
      }
    } else if (g_robotStateData.status == 103) { // 消息ID重复状态
      if (g_new_message_flag) {
        // 收到新消息。我们不做任何判断。
        // 简单地将状态恢复为空闲(1)，让下一个主循环去重新评估
        // 当前 g_robotControlData 中的消息是好是坏。
        // 注意：这里不再消耗 g_new_message_flag，而是留给 status=1
        // 的逻辑去处理。
        printf("收到新指令，准备从重复ID状态(103)中恢复并重新评估...\n");
        g_robotStateData.status = 1; // 恢复到空闲状态，以便下次循环时重新检查
      }
    } else if (g_robotStateData.status == 104) { // 超出范围状态
      if (g_new_message_flag) {
        printf("收到新指令，准备从范围错误(104)中恢复并重新评估...\n");
        g_robotStateData.status = 1;
      }
    } else if (g_robotStateData.status == 0) { // 任务执行中
      if (g_new_message_flag) {
        g_new_message_flag = false;
        if (strcasecmp(g_robotControlData.move_type, "halt") == 0 &&
            g_robotControlData.axis_num == 29) {
          printf("\n========== 执行停止命令 ==========\n");
          pthread_mutex_lock(&cmdSet_mutex);
          cmdSet.target_pos[0] = cmdSet.actual_pos[0];
          pthread_mutex_unlock(&cmdSet_mutex);
          g_robotStateData.status = 1;
          if (strlen(last_msg_id) > 0)
            mqtt_send_ack(last_msg_id, 1);
          printf("停止命令执行完成，进入空闲状态\n");
        } else if (strcasecmp(g_robotControlData.move_type,
                              "disable_voltage") == 0 &&
                   g_robotControlData.axis_num == 29) {
          printf("\n========== 执行急停命令（运行中） ==========\n");
          g_robotStateData.status = 300;
        } else if (strlen(g_robotControlData.msg_id) > 0 &&
                   strlen(last_msg_id) > 0 &&
                   strcmp(g_robotControlData.msg_id, last_msg_id) != 0) {
          printf("警告: 设备正在运行，收到新命令 msg_id=%s (应答status=105)\n",
                 g_robotControlData.msg_id);
          mqtt_send_ack(g_robotControlData.msg_id, 105);
        }
      }

      double error = last_pos_um - g_robotStateData.parameter;
      if (-10.0 < error && error < 10.0) {
        printf("\n========== 任务完成 ==========\n");
        if (strlen(last_msg_id) > 0)
          printf("msg_id: %s\n", last_msg_id);
        printf("到达位置: %f µm\n", g_robotStateData.parameter);
        printf("==============================\n\n");
        g_robotStateData.status = 1;
        if (strlen(last_msg_id) > 0)
          mqtt_send_ack(last_msg_id, 1);
      }
    } else if (g_robotStateData.status == 300) { // 急停状态
      if (g_new_message_flag) {
        g_new_message_flag = false;
        printf("警告: 设备处于急停状态(300)，忽略新指令: move_type='%s'\n",
               g_robotControlData.move_type);
      }
    }

    usleep(4000);
  }
  return NULL;
}

int term_init() {
  pthread_t process_thread;
  pthread_create(&process_thread, NULL, process_main, NULL);
  pthread_detach(process_thread);
  return 0;
}